/*
 * Communication.h
 *
 *  Created on: Apr 22, 2011
 *      Author: tflanzer
 */

#ifndef FILTER_H_
#define FILTER_H_

#define numStates 24
#define gpsSize 5
#define imuSize 8

#include <cstdio>
#include <list>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "mavlink.h"
#include "Communication.h"
#include "Matrix.h"
#include "execStruct.h"
#include "Aircraft.h"
using namespace std;

/**
 * uses fortran BLAS and LAPACK libraries
 */
extern "C"{

// Compute eigenvalues
void dsyevr_(const char *jobz, const char *range, const char *uplo, const int *n,
		double *A, const int *lda, const int *vl, const int *vu, const int *il,
		const int *iu, const double *abstol, int *m, double *w,
		double *z, const int *ldz, int *isuppz, double *work, const int *lwork,
        int *iwork, const int* liwork, const int* info);

// Compute machine precision
double dlamch_( const char *cmach );

}

void computeMatrixSquareRoot(double *A, double *B, int N);

class Filter: public Communication::AUPListener {

public:

	double dt;
	Aircraft *ac;

	Filter(Aircraft* aci);
	virtual ~Filter();
	void AUPdataReceived(Communication &com);
	
	// Methods
	void getCurrentState(double *xcurr);
	void generateSigmaPointsProcess(double *sigmaPts, double *xcurr);
	void generateSigmaPointsMeasurement(double *sigmaPts, double *mubt, double *Sbart);
	void propagatePtsThroughProcessModel(double *xprocess, double *sigmaPts);
	void computePredictedMean(double *xprocess, double *mubt);
	void computePredictedVariance(double *xprocess, double *mubt, double *Smxk);
	void calcualteSigmaPtsForPrediction();
	void computePredictedObservation(double *xmeas, double *sigmaPts, bool flag);
	void computePredictedMeasurement(double *xmeas, double *zbt, bool flag);
	void computePredictedMeasurementUncertainty(double *xmeas, double *zbt,
			double *St, bool flag);
	void computeCrossCovariance(double *xprocess, double *xmeas, double *mubt,
			double *zbt, double *Scc, bool flag);
	void computeKalmanGain(double *St, double *Scc, double *Kt, bool flag);
	void computeNewMeanandCovariance(double *mubt, double *Kt, double *zbt,
			double *Sbart, double *St, bool flag);
	void updateState(double *mut);

private:

	// Initial Covariance Estimate
	double P[numStates][numStates]; 	// State covariance estimate
	double Q_IMU_GPS[gpsSize+imuSize];	// GPS + IMU measurement noise (uncorrelated)
	double Q_IMU[imuSize];		// IMU measurement noise (uncorrelated)
	double R[numStates]; 		// Process model noise (uncorrelated)

	// UKF Parameters
	double alpha, beta, kappa;	// Inputs
	double lambda, gamma;		// Calculated

	// Sigma Point Weights
	double wm[2*numStates+1];
	double wc[2*numStates+1];
};

#endif /* FILTER_H_ */
